/// 三角面片化
#include <iostream>

#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/impl/mls.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>


typedef pcl::PointXYZRGB PointT;

using namespace std;

void help(char *appname) {
    printf("Usage: %s <inputfile.pcd> <outputlfolder>\n", appname);
}

int main(int argc, char *argv[])
{
    if (argc < 3) {
        help(argv[0]);
        return -1;
    }

    string inputFile = argv[1];
    string outputFolder = argv[2];

    if (0 != access(outputFolder.c_str(), W_OK)) {
        mkdir(outputFolder.c_str(), 0777);
    }

    // Load input file
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_downSampled(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_smoothed(new pcl::PointCloud<PointT>);
    if (pcl::io::loadPCDFile(inputFile, *cloud) == -1)
    {
        cout << "点云数据读取失败！" << endl;
    }

    std::cout << "Orginal points number: " << cloud->points.size() << std::endl;

    // 下采样
    pcl::VoxelGrid<PointT> downSampled;  //创建滤波对象
    downSampled.setInputCloud(cloud);            //设置需要过滤的点云给滤波对象
    downSampled.setLeafSize(0.0005f, 0.0005f, 0.0005f);  //设置滤波时创建的体素体积为1cm的立方体
    downSampled.filter(*cloud_downSampled);           //执行滤波处理，存储输出
    std::cout << "cloud_downSampled: " << cloud_downSampled->size() << std::endl;

    //统计滤波
    pcl::StatisticalOutlierRemoval<PointT> statisOutlierRemoval;       // 统计滤波创建滤波器对象
    statisOutlierRemoval.setInputCloud(cloud_downSampled);            //设置待滤波的点云
    statisOutlierRemoval.setMeanK(50);                                //设置在进行统计时考虑查询点临近点数
    statisOutlierRemoval.setStddevMulThresh(2.0);                     //设置判断是否为离群点的阀值:均值+1.0*标准差
    statisOutlierRemoval.filter(*cloud_filtered);                     //滤波结果存储到cloud_filtered
    std::cout << "cloud_filtered: " << cloud_filtered->size() << std::endl;

    /*
    pcl::RadiusOutlierRemoval<PointT> outrem;       // 半径滤波创建滤波器对象
    outrem.setInputCloud(cloud_downSampled);        //设置输入点云
    outrem.setRadiusSearch(0.2);        //设置在0.8的半径范围内找近邻点
    outrem.setMinNeighborsInRadius(30);  //设置查询查询点的近邻点集数小于2的删除
    outrem.filter(*cloud_filtered);     //执行滤波，结果保存在cloud_filter,apply filter
    //pcl::io::savePCDFile("C:/Users/lrk/Desktop/test_data/new4_13/point_pcd/m111filteredPC.pcd", *cloud_filtered);
    */

    // 对点云重采样
    pcl::search::KdTree<PointT>::Ptr treeSampling(new pcl::search::KdTree<PointT>);// 创建用于最近邻搜索的KD-Tree
    pcl::PointCloud<PointT> mls_point;    //输出MLS
    pcl::MovingLeastSquares<PointT, PointT> mls; // 定义最小二乘实现的对象mls
    mls.setComputeNormals(false);  //设置在最小二乘计算中是否需要存储计算的法线
    mls.setInputCloud(cloud_filtered);         //设置待处理点云
    mls.setPolynomialOrder(3);            // 拟合2阶多项式拟合
    //mls.setPolynomialFit(false);     // 设置为false可以 加速 smooth
    mls.setPolynomialOrder(false);
    mls.setSearchMethod(treeSampling);         // 设置KD-Tree作为搜索方法
    mls.setSearchRadius(0.0005);           // 单位m.设置用于拟合的K近邻半径
    mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB>::SAMPLE_LOCAL_PLANE);//
    /*
    a)       NONE——将不会进行upsampling
    b)       SAMPLE_LOCAL_PLANE——这个方法就是参考论文中采用的方法，当然此方法所需的计算强度也相当庞大。若使用此方法，将需要调用两个函数：
    A.       Inline voidsetUpsamplingRadius(double radius) 此函数规定了点云增长的区域。可以这样理解：把整个点云按照此半径划分成若干个子点云，然后一一索引进行点云增长。
    B.       Inline voidsetUpsamlingStepSize(double size) 对于每个子点云处理时迭代的步长。
    c)       RANDOM_UNIFORM_DENSITY——也是使用上面子点云的原理，只不过它使得稀疏区域的密度增加，从而使得整个点云的密度均匀。它需要调用函数：
             inline void setPointDensity(int desired_num_po-ints_in_radius) 注意此函数输入整型变量，意为半径内点的个数。（这个半径应该是search的半径，不需要重新设置）。
    d)       VOXEL_GRID_DILATION——这个方法有两个步骤：首先将点云以voxels分割，然后进行迭代使得voxels的数目增加。它的结果是：填充空洞和平均化点云的密度。它需要调用的函数为
    A.       Inline voidsetDilationVoxelSize(float voxel_size) 设定voxel的大小。
    B.       Inline voidsetDilationIterations(int iterations) 设置迭代的次数。
    */

    mls.setUpsamplingRadius(0.001);// 设置采样的半径
    mls.setUpsamplingStepSize(0.001);// 采样步长的大小
    mls.process(mls_point);
    // 输出重采样结果
    // cloud_smoothed = mls_point.makeShared();
    cloud_smoothed = cloud_filtered;
    std::cout << "cloud_smoothed: " << cloud_smoothed->size() << std::endl;

    // 法线估计
    pcl::NormalEstimation<PointT, pcl::Normal> normalEstimation;             //创建法线估计的对象
    normalEstimation.setInputCloud(cloud_smoothed);                         //输入点云
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);// 创建用于最近邻搜索的KD-Tree
    normalEstimation.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 定义输出的点云法线
    // K近邻确定方法，使用k个最近点，或者确定一个以r为半径的圆内的点集来确定都可以，两者选1即可
    normalEstimation.setKSearch(10);// 使用当前点周围最近的10个点
    //normalEstimation.setRadiusSearch(0.0008);            //对于每一个点都用半径为3cm的近邻搜索方式
    normalEstimation.compute(*normals);               //计算法线
    // 输出法线
    std::cout << "normals: " << normals->size() << ", " << "normals fields: " << pcl::getFieldsList(*normals) << std::endl;
    // 将点云位姿、颜色、法线信息连接到一起
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::concatenateFields(*cloud_smoothed, *normals, *cloud_with_normals);


    // 贪心投影三角化
    //定义搜索树对象?
    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
    tree2->setInputCloud(cloud_with_normals);
    // 三角化
    pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;   // 定义三角化对象
    pcl::PolygonMesh triangles; //存储最终三角化的网络模型

    // 设置三角化参数
    gp3.setSearchRadius(0.001);  //设置搜索时的半径，也就是KNN的球半径,该函数设置了三角化后得到的每个三角形的最大可能边长；
    gp3.setMu(2.5);  //设置样本点搜索其近邻点的最远距离为2.5倍（典型值2.5-3），这样使得算法自适应点云密度的变化
    gp3.setMaximumNearestNeighbors(100);    //设置样本点最多可搜索的邻域个数，典型值是50-100

    gp3.setMinimumAngle(M_PI / 18); // 设置三角化后得到的三角形内角的最小的角度为10°
    gp3.setMaximumAngle(2 * M_PI / 3); // 设置三角化后得到的三角形内角的最大角度为120°

    gp3.setMaximumSurfaceAngle(M_PI / 4); // 设置某点法线方向偏离样本点法线的最大角度45°，如果超过，连接时不考虑该点
    gp3.setNormalConsistency(false);  //设置该参数为true保证法线朝向一致，设置为false的话不会进行法线一致性检查

    gp3.setInputCloud(cloud_with_normals);     //设置输入点云为有向点云
    gp3.setSearchMethod(tree2);   //设置搜索方式
    gp3.reconstruct(triangles);  //重建提取三角化
    //pcl::io::saveVTKFile("C:/Users/lrk/Desktop/test_data/new4_13/point_pcd/m111mesh.vtk", triangles);
    pcl::io::savePLYFile(outputFolder + "/m66.ply", triangles);

    // 显示网格化结果
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0.8, 0.1, 0.1);  //
    viewer->addPolygonMesh(triangles, "wangge");  //
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}
